/* Copyright 2019 SiFive, Inc */
/* SPDX-License-Identifier: Apache-2.0 */



/******************************************************************************
 *
 * main() creates one queue, and two tasks.  It then starts the
 * scheduler.
 *
 * The Queue Send Task:
 * The queue send task is implemented by the prvQueueSendTask() function in
 * this file.  prvQueueSendTask() sits in a loop that causes it to repeatedly
 * block for 1000 milliseconds, before sending the value 100 to the queue that
 * was created within main().  Once the value is sent, the task loops
 * back around to block for another 1000 milliseconds...and so on.
 *
 * The Queue Receive Task:
 * The queue receive task is implemented by the prvQueueReceiveTask() function
 * in this file.  prvQueueReceiveTask() sits in a loop where it repeatedly
 * blocks on attempts to read data from the queue that was created within
 * blinky().  When data is received, the task checks the value of the
 * data, and if the value equals the expected 100, writes 'Blink' to the UART
 * (the UART is used in place of the LED to allow easy execution in QEMU).  The
 * 'block time' parameter passed to the queue receive function specifies that
 * the task should be held in the Blocked state indefinitely to wait for data to
 * be available on the queue.  The queue receive task will only leave the
 * Blocked state when the queue send task writes to the queue.  As the queue
 * send task writes to the queue every 1000 milliseconds, the queue receive
 * task leaves the Blocked state every 1000 milliseconds, and therefore toggles
 * the LED every 1 second.
 */

/* Standard includes. */
#include <stdio.h>
#include <string.h>
#include <unistd.h>

/* Kernel includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

/* Freedom metal includes. */
#include <metal/machine.h>
#include <metal/machine/platform.h>

#include <metal/lock.h>
#include <metal/uart.h>
#include <metal/interrupt.h>
#include <metal/clock.h>
#include "motor.h"
#include <metal/i2c.h>

extern struct motor Motor;
extern struct LSM303 lsm303;

/* Priorities used by the tasks. */
#if( portUSING_MPU_WRAPPERS == 1 )
/* Adding portPRIVILEGE_BIT on task priority permit to this task to be executed into privilleged mode
 * if not, the task will not have access to the global variable. if you need to have access to global 
 * variable, you need to define region and change the function to call to create a task. in order to
 * avoid confusion, it is made in a different example (see example-freertos-blinky-pmp)
 */
#else
#define	mainLED_TASK_PRIORITY		( tskIDLE_PRIORITY + 1 )
#define	mainBUTTON_TASK_PRIORITY		( tskIDLE_PRIORITY + 3 )
#define	mainCHECKETLINE_TASK_PRIORITY		( tskIDLE_PRIORITY + 4 )
#define	mainUPDATEMOTOR_TASK_PRIORITY		( tskIDLE_PRIORITY + 3 )
#define	mainACCELER_TASK_PRIORITY		( tskIDLE_PRIORITY + 4 )
#endif

/* The 1s value is converted to ticks using the pdMS_TO_TICKS() macro. */
#define mainQUEUE_TICK_COUNT_FOR_1S			pdMS_TO_TICKS( WAIT_MS )

/* The maximum number items the queue can hold.  The priority of the receiving
task is above the priority of the sending task, so the receiving task will
preempt the sending task and remove the queue items each time the sending task
writes to the queue.  Therefore the queue will never have more than one item in
it at any time, and even with a queue length of 1, the sending task will never
find the queue full. */
#define mainQUEUE_LENGTH					( 1 )

/*---------iic-------------------------------------------------*/
#define COMPASS_I2C_ADDR 0b0011110
#define ACCELEROMETER_I2C_ADDR 0b0011001

#define I2C_BAUDRATE 400000


/*-----------------------------------------------------------*/
/*
 * Functions:
 * 		- prvSetupHardware: Setup Hardware according CPU and Board.
 */
static void prvSetupHardware( void );

/*
 * The tasks as described in the comments at the top of this file.
 */

static void prvLedTask(void *pvParameters);
static void prvButtonTask(void *pvParameters);
static void prvCheckLineTask(void *pvParameters);
static void prvUpdateMotorTask(void *pvParameters);
static void prvAccelerTask(void *pvParameters);
/*-----------------------------------------------------------*/
/*
*  init funtion
*/
void button_init();
void line_sensor_init();

/*
 * funtion
 */
void sensor_data_process();

/*------------------------------------------------------------*/
/* The queue used by both tasks. */
static QueueHandle_t xQueue = NULL;

struct metal_gpio *gpio0;
struct metal_cpu *cpu0;
struct metal_interrupt *cpu_intr,*gpio_intr;
struct metal_led *led0_red, *led0_green, *led0_blue;
struct metal_i2c *i2c;

/*-----------------------------------------------------------*/

int main( void )
{

	TaskHandle_t xHandle_ReceiveTask, xHandle_SendTask,xHandle_LedTask,
		xHandle_ButtonTask,xHandle_CheckLineTask,xHandle_UpdateMotorTask,
		xHandle_AccelerTask;
	const char * const pcMessage = "FreeRTOS start\r\n";
	const char * const pcMessageEnd = "FreeRTOS Demo end\r\n";

	prvSetupHardware();
	write( STDOUT_FILENO, pcMessage, strlen( pcMessage ) );
	
	/* Create the queue. */
	xQueue = xQueueCreate( mainQUEUE_LENGTH, sizeof( uint32_t ) );

	if( xQueue != NULL )
	{
		/* Start the two tasks as described in the comments at the top of this
		file. */

		xTaskCreate( prvLedTask,				/* The function that implements the task. */
					"led", 					/* The text name assigned to the task - for debug only as it is not used by the kernel. */
					configMINIMAL_STACK_SIZE, 		/* The size of the stack to allocate to the task. */
					NULL, 					/* The parameter passed to the task - not used in this case. */
					mainLED_TASK_PRIORITY, 	/* The priority assigned to the task. */
					&xHandle_LedTask );	
		xTaskCreate( prvAccelerTask,				/* The function that implements the task. */
					"acceler", 					/* The text name assigned to the task - for debug only as it is not used by the kernel. */
					configMINIMAL_STACK_SIZE, 		/* The size of the stack to allocate to the task. */
					NULL, 					/* The parameter passed to the task - not used in this case. */
					mainACCELER_TASK_PRIORITY, 	/* The priority assigned to the task. */
					&xHandle_AccelerTask );
		/* Start the tasks and timer running. */
		vTaskStartScheduler();

	/* If all is well, the scheduler will now be running, and the following
	line will never be reached.  If the following line does execute, then
	there was insufficient FreeRTOS heap memory available for the Idle and/or
	timer tasks to be created. 
	or task have stoppped the Scheduler */

		vTaskDelete( xHandle_SendTask );
		vTaskDelete( xHandle_ReceiveTask );
	}

	write( STDOUT_FILENO, pcMessageEnd, strlen( pcMessageEnd ) );

}
/*-----------------------------------------------------------*/
static void prvUpdateMotorTask(void *pvParameters)
{
	TickType_t xNextWakeTime;
	(void)pvParameters;

	xNextWakeTime = xTaskGetTickCount();
	while(1){
		motor_run();
		vTaskDelayUntil( &xNextWakeTime, pdMS_TO_TICKS( 300 ) );
	}
}
/*-----------------------------------------------------------*/
/*
 * out_addr 是加速度计x,z轴高位输出寄存器
 * buff用来缓存读取的数据
 * reg1的第一元素是reg1_a寄存器的地址，该寄存器配置工作模式，输出速率；第二个元素是写入寄存器的值
 */
static void prvAccelerTask(void *pvParameters)
{
	TickType_t xNextWakeTime;
	const char * const pcMessage = "ACCELER\r\n";
	(void)pvParameters;

	static unsigned char out_addr[2]={0x29,0x2d},buff[2]={0},reg1[2]={0x20,0x77};
	//延时时间，同时也是时间的微分
	Motor.dt=200;
	xNextWakeTime = xTaskGetTickCount();
	metal_i2c_init(i2c, I2C_BAUDRATE, METAL_I2C_MASTER);
	//配置为普通模式，400hz输出，使能x，y，z
	metal_i2c_write(i2c, ACCELEROMETER_I2C_ADDR, 2,reg1 , METAL_I2C_STOP_ENABLE);
	while(1){
		write(STDOUT_FILENO,pcMessage,strlen(pcMessage));
		taskENTER_CRITICAL();
		metal_i2c_write(i2c, ACCELEROMETER_I2C_ADDR, 1,out_addr , METAL_I2C_STOP_DISABLE);
		metal_i2c_read(i2c, ACCELEROMETER_I2C_ADDR, 1,buff , METAL_I2C_STOP_ENABLE);
		metal_i2c_write(i2c, ACCELEROMETER_I2C_ADDR, 1,&out_addr[1] , METAL_I2C_STOP_DISABLE);
		metal_i2c_read(i2c, ACCELEROMETER_I2C_ADDR, 1,&buff[1] , METAL_I2C_STOP_ENABLE);
		lsm303.acc_x_raw=buff[0];
		lsm303.acc_z_raw=buff[1];
		//对原生数据处理，转化成加速度，速度，位移
		sensor_data_process();
		taskEXIT_CRITICAL();
		vTaskDelayUntil( &xNextWakeTime, pdMS_TO_TICKS( Motor.dt ) );
	}
}
/*-----------------------------------------------------------*/
static void prvCheckLineTask(void *pvParameters)
{
	TickType_t xNextWakeTime;
	(void)pvParameters;
	xNextWakeTime = xTaskGetTickCount();
	while(1){

		if(metal_gpio_get_input_pin(gpio0,9))
		{
			Motor.left_cmp=250;
			Motor.right_cmp=180;
		}

		else if(metal_gpio_get_input_pin(gpio0,20))
		{
			Motor.left_cmp=180;
			Motor.right_cmp=250;
		}
		else {
			Motor.left_cmp=180;
			Motor.right_cmp=180;
		}		
		vTaskDelayUntil( &xNextWakeTime, pdMS_TO_TICKS( 300 ) );
	}
}
/*-----------------------------------------------------------*/
static void prvButtonTask(void *pvParameters)
{
	TickType_t xNextWakeTime;
	(void)pvParameters;
	const char * const pcMessage = "button press\r\n";
	xNextWakeTime = xTaskGetTickCount();
	while(1){
		if(metal_gpio_get_input_pin(gpio0,0)==0){
			write(STDOUT_FILENO,pcMessage,strlen(pcMessage));
		}

		vTaskDelayUntil( &xNextWakeTime, pdMS_TO_TICKS( 100 ) );
	}
}
/*-----------------------------------------------------------*/
static void prvLedTask(void *pvParameters)
{
	TickType_t xNextWakeTime;
	(void)pvParameters;
	const char * const pcMessage = "led task\r\n";
	xNextWakeTime = xTaskGetTickCount();
	while(1){
		write(STDOUT_FILENO,pcMessage,strlen(pcMessage));
		vTaskDelayUntil( &xNextWakeTime, pdMS_TO_TICKS( 1000 ) );
	}
}

/*-----------------------------------------------------------*/
//硬件初始化函数，在任务启动前完成
static void prvSetupHardware( void )
{
	const char * const pcWarningMsg = "hardware set up.\n";

	cpu0 = metal_cpu_get(metal_cpu_get_current_hartid());
	cpu_intr = metal_cpu_interrupt_controller(cpu0);
	gpio0=metal_gpio_get_device(0);
	i2c = metal_i2c_get_device(0);
	//gpio_intr=metal_gpio_interrupt_controller(gpio0);


	metal_interrupt_enable(cpu_intr, 0);
	write( STDOUT_FILENO, pcWarningMsg, strlen( pcWarningMsg ) );

}
/*-----------------------------------------------------------*/
void sensor_data_process()
{
	unsigned char temp[2]={0};
	//获取原码数据
	if(lsm303.acc_x_raw>>7){
		temp[0]=(~lsm303.acc_x_raw);
		temp[0]|=(1<<7);
	}
	else {
		temp[0]=(~lsm303.acc_x_raw);
		temp[0]&=~(1<<7);
	}
	if(lsm303.acc_z_raw>>7){
		temp[1]=(~lsm303.acc_z_raw);
		temp[1]|=(1<<7);
	}
	else {
		temp[1]=(~lsm303.acc_z_raw);
		temp[1]&=~(1<<7);
	}
	lsm303.acc_x_comple=temp[0];
	lsm303.acc_z_comple=temp[1];
	//剔除重力分量的影响
	temp[0]=77-temp[0];
	temp[1]=-(87+temp[1]);
	//滤掉微小的抖动
	temp[0]&=~0x07;
	temp[1]&=~0x07;

	lsm303.acc_x_filter=temp[0];
	lsm303.acc_z_filter=temp[1];

	//乘以三角系数得到水平方向的加速度
	//sinθ=0.66 cosθ=0.75
	Motor.acc=(temp[0]*0.66+temp[1]*0.75)*4*39;//100ug
	//加速度积分
	Motor.speed+=Motor.acc*Motor.dt;//10^(-7)m/s
	//速度积分
	Motor.distance+=Motor.speed*Motor.dt;
}
void button_init()
{
	metal_gpio_disable_output(gpio0,0);
	metal_gpio_enable_input(gpio0,0);
	metal_gpio_disable_pinmux(gpio0,0);
}
void line_sensor_init()
{
	metal_gpio_disable_output(gpio0,9);
	metal_gpio_enable_input(gpio0,9);
	metal_gpio_disable_output(gpio0,20);
	metal_gpio_enable_input(gpio0,20);
}
void vApplicationMallocFailedHook( void )
{
	/* vApplicationMallocFailedHook() will only be called if
	configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h.  It is a hook
	function that will get called if a call to pvPortMalloc() fails.
	pvPortMalloc() is called internally by the kernel whenever a task, queue,
	timer or semaphore is created.  It is also called by various parts of the
	demo application.  If heap_1.c or heap_2.c are used, then the size of the
	heap available to pvPortMalloc() is defined by configTOTAL_HEAP_SIZE in
	FreeRTOSConfig.h, and the xPortGetFreeHeapSize() API function can be used
	to query the size of free heap space that remains (although it does not
	provide information on how the remaining heap might be fragmented). */
	taskDISABLE_INTERRUPTS();

	if ( led0_red != NULL )
	{
		// Red light on
		metal_led_off(led0_red);
	}

	_exit(1);
}
/*-----------------------------------------------------------*/

void vApplicationIdleHook( void )
{
	/* vApplicationIdleHook() will only be called if configUSE_IDLE_HOOK is set
	to 1 in FreeRTOSConfig.h.  It will be called on each iteration of the idle
	task.  It is essential that code added to this hook function never attempts
	to block in any way (for example, call xQueueReceive() with a block time
	specified, or call vTaskDelay()).  If the application makes use of the
	vTaskDelete() API function (as this demo application does) then it is also
	important that vApplicationIdleHook() is permitted to return to its calling
	function, because it is the responsibility of the idle task to clean up
	memory allocated by the kernel to any task that has since been deleted. */
}
/*-----------------------------------------------------------*/

void vApplicationStackOverflowHook( TaskHandle_t pxTask, char *pcTaskName )
{
	( void ) pcTaskName;
	( void ) pxTask;

	/* Run time stack overflow checking is performed if
	configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2.  This hook
	function is called if a stack overflow is detected. */
	taskDISABLE_INTERRUPTS();

	write( STDOUT_FILENO, "ERROR Stack overflow on func: ", 30 );
	write( STDOUT_FILENO, pcTaskName, strlen( pcTaskName ) );


	if ( led0_red != NULL )
	{
		// Red light on
		metal_led_off(led0_red);
	}

	_exit(1);
}
/*-----------------------------------------------------------*/

void vApplicationTickHook( void )
{
	/* The tests in the full demo expect some interaction with interrupts. */
}
/*-----------------------------------------------------------*/

void vAssertCalled( void )
{
	taskDISABLE_INTERRUPTS();

	if ( led0_red != NULL )
	{
		// Red light on
		metal_led_off(led0_red);
	}

	_exit(1);
}
